/*
 *   Copyright (C) 2011 David Swords
 *	
 *   This program is free software: you can redistribute it and/or modify
 *   it under the terms of the GNU General Public License as published by
 *   the Free Software Foundation, either version 3 of the License, or
 *   (at your option) any later version.
 *
 *   This program is distributed in the hope that it will be useful,
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *   GNU General Public License for more details.
 *
 *   You should have received a copy of the GNU General Public License
 *   along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

import sensor.SensorReader;
import locate.Pose;
import motion.MotionController;

public class LANdroid {

	private final String class_details = "LANdroid.";
	private final int ID;
	private Pose c_pose;
	private MotionController mc;
	private SensorReader sr;
	
	public LANdroid(final int number, final String IP) {
		final String constructor_details = class_details + "LANdroid(int, Vector< Double >)";
		chk(!(number >= 0 && number <= 9), "LANdroid ID outside of range. 0 to 9. Was " + number, constructor_details);
		chk(IP.isEmpty(), "Given IP was blank.", constructor_details);
		ID = number;
		c_pose = new Pose(0.0, 0.0, 0.0);
		mc = new MotionController(IP);
		sr = new SensorReader(IP);
	}
	
	private void chk(final boolean expression, final String message, final String location) {
		if(expression) {
			System.err.println("#############################################################");
			System.err.println(message);
			System.err.println(location);
			System.err.println("#############################################################");
			System.exit(1);
		}
	}
	
	public void move(final long time, double radian) {
		final String method_details = class_details + "move(double, double) return void";
		chk(!(radian >= 0 && radian <= 2*Math.PI), "The double t is outside of 0 to 2*PI range with " + radian, method_details);
		chk(mc == null, "The MotionController mc is equal to null, please initialise.", method_details);
		
		direction(radian);
		forward_d(time);
	}
	
	public void forward() {
		final String method_details = class_details + "forward() returns void";
		chk(mc == null, "The MotionController mc is equal null, please initialize.", method_details);
		chk(!mc.isConnected(), "MotionController is not connected.", method_details);
		mc.forward();
	}
	
	public void forward_d(final double distance) {
		final String method_details = class_details + "forward(double) returns void";
		chk(mc == null, "The MontionController mc is equal null, please initialize.", method_details);
		chk(distance < 0, "Negative distance value given, cannot comply, was " + distance, method_details);
		mc.forward_d(distance);
	}
	
	public void forward_t(final long time) {
		final String method_details = class_details + "forward(double) returns void";
		chk(mc == null, "The MontionController mc is equal null, please initialize.", method_details);
		chk(time < 0, "Negative distance value given, cannot comply, was " + time, method_details);
		mc.forward_t(time);
	}
	
	public void backward() {
		final String method_details = class_details + "backward() returns void";
		chk(mc == null, "The MotionController mc is equal null, please initialize.", method_details);
		chk(!mc.isConnected(), "MotionController is not connected.", method_details);
		mc.backward();
	}
	
	public void backward_d(final double distance) {
		final String method_details = class_details + "backward_d(double) returns void";
		chk(mc == null, "The MontionController mc is equal null, please initialize.", method_details);
		chk(distance < 0, "Negative distance value given, cannot comply, was " + distance, method_details);
		mc.backward(distance);
	}
	
	public void backward_t(final long time) {
		final String method_details = class_details + "backward_t(double) returns void";
		chk(mc == null, "The MontionController mc is equal null, please initialize.", method_details);
		chk(time < 0, "Negative time value given, cannot comply, was " + time, method_details);
		mc.backward(time);
	}
	
	public void right() {
		final String method_details = class_details + "right() returns void";
		chk(mc == null, "The MotionController mc is equal null, please initialize.", method_details);
		mc.right();
	}
	
	public void left() {
		final String method_details = class_details + "left() returns void";
		chk(mc == null, "The MotionController mc is equal null, please initialize.", method_details);
		mc.left();
	}
	
	public void direction(final double radian) {
		final String method_details = class_details + "direction(double) returns void";
		chk(mc == null, "The MotionController mc is equal null, please initialize.", method_details);
		chk(!(radian >=0 || radian <= 2*Math.PI), "The double t is outside of 0 to 2*PI range with " + radian, method_details);
		mc.direction(radian);
	}
	
	public void flipperClock() {
		final String method_details = class_details + "flipperClock() returns void";
		chk(mc == null, "The MotionController mc is equal null, please initialize.", method_details);
		mc.flipperClock();
	}
	
	public void flipperAntiClock() {
		final String method_details = class_details + "flipperAntiClock() returns void";
		chk(mc == null, "The MotionController mc is equal null, please initialize.", method_details);
		mc.flipperAntiClock();
	}
	
	public void flipper(final double radian) {
		final String method_details = class_details + "flipper(double) return void";
		chk(!(radian >= 0 && radian <= 2*Math.PI), "The double t is outside of 0 to 2*PI range with " + radian, method_details);
		chk(mc == null, "The MotionController mc is equal to null, please initialise.", method_details);
		mc.flipperPosition(radian);
	}
	
	public void flipperReset() {
		final String method_details = "flipperReset() returns void";
		chk(mc == null, "The MotionController mc is equal to null, please initialise.", method_details);
		mc.flipperPosition(0);
	}
	
	public void stop() {
		final String method_details = "stop() returns void";
		chk(mc == null, "The MotionController mc is equal to null, please initialise.", method_details);
		mc.stop();
	}
	
	public Pose pose() {
		final String method_details =  class_details + "pose() returns Pose";
		chk(sr == null, "The SensorReader sr is equal to null, please initialise.", method_details);
		c_pose = sr.pose();
		chk(c_pose == null, "The Pose c_pose is equal to null, please initialise.", method_details);
		return c_pose;
	}
	
	public int ID() {
		final String method_details = class_details + "ID() returns Pose";
		chk(!(ID >= 0 && ID <= 9), "LANdroid ID outside of range. 0 to 9. Was " + ID, method_details);
		return ID;
	}

	public boolean isConnected() {
		// TODO Auto-generated method stub
		return false;
	}
}
